
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode.c
  * @author     baiyang
  * @date       2023-7-5
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"

#include <rtthread.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mode_ctor(mode_base_t mode, mode_ops_t ops)
{
    mode->ops = ops;

    mode->ahrs        = fms.ahrs;
}

void mode_exit(mode_base_t mode)
{
    // call sub-classes exit
    mode_exit2(mode);

#if 0 // TODO: ?
    // stop autotuning if not AUTOTUNE mode
    if (plane.control_mode != &plane.mode_autotune){
        plane.autotune_restore();
    }
#endif
}

bool mode_enter(mode_base_t mode)
{
    // cancel inverted flight
    fms.auto_state.inverted_flight = false;
    
    // cancel waiting for rudder neutral
    fms.takeoff_state.waiting_for_rudder_neutral = false;

    // don't cross-track when starting a mission
    fms.auto_state.next_wp_crosstrack = false;

    // reset landing check
    fms.auto_state.checked_for_autoland = false;

    // zero locked course
    fms.steer_state.locked_course_err = 0;
    fms.steer_state.locked_course = false;

    // reset crash detection
    fms.crash_state.is_crashed = false;
    fms.crash_state.impact_detected = false;

    // reset external attitude guidance
    fms.guided_state.last_forced_rpy_ms[0] = 0;
    fms.guided_state.last_forced_rpy_ms[1] = 0;
    fms.guided_state.last_forced_rpy_ms[2] = 0;

    fms.guided_state.last_forced_throttle_ms = 0;
    fms.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range
    fms.guided_state.target_heading_type = GUIDED_HEADING_NONE;
    fms.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane.
    fms.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane.
    fms.guided_state.last_target_alt = 0;

    // zero initial pitch and highest airspeed on mode change
    fms.auto_state.highest_airspeed = 0;
    fms.auto_state.initial_pitch_cd = fms.ahrs->pitch_sensor_cd;

    // disable taildrag takeoff on mode change
    fms.auto_state.fbwa_tdrag_takeoff_mode = false;

    // start with previous WP at current location
    fms.prev_WP_loc = fms.current_loc;

    // new mode means new loiter
    fms.loiter.start_time_ms = 0;

    // record time of mode change
    fms.last_mode_change_ms = time_millis();

    // set VTOL auto state
    fms.auto_state.vtol_mode = mode_is_vtol_mode(mode);
    fms.auto_state.vtol_loiter = false;

    // initialize speed variable used in AUTO and GUIDED for DO_CHANGE_SPEED commands
    fms.new_airspeed_cm = -1;

#if HAL_QUADPLANE_ENABLED
    quadplane.mode_enter();
#endif

    bool enter_result = mode_enter2(mode);

    if (enter_result) {
        // -------------------
        // these must be done AFTER _enter() because they use the results to set more flags

        // start with throttle suppressed in auto_throttle modes
        fms.throttle_suppressed = mode_does_auto_throttle(mode);

        // reset steering integrator on mode change
        sctrl_reset_i(&fms.steer_controller);

        // update RC failsafe, as mode change may have necessitated changing the failsafe throttle
        fms_control_failsafe();
    }

    return enter_result;
}

// returns true if the vehicle can be armed in this mode
bool mode_pre_arm_checks(mode_base_const_t mode, size_t buflen, char *buffer)
{
    if (!mode_pre_arm_checks2(mode, buflen, buffer)) {
        if (rt_strlen(buffer) == 0) {
            // If no message is provided add a generic one
            rt_snprintf(buffer, buflen, "mode not armable");
        }
        return false;
    }

    return true;
}

// Reset rate and steering controllers
void mode_reset_controllers(mode_base_t mode)
{
    (void)mode;

    // reset integrators
    rctrl_reset_i(&fms.roll_controller);
    pctrl_reset_i(&fms.pitch_controller);
    yctrl_reset_i(&fms.yaw_controller);

    // reset steering controls
    fms.steer_state.locked_course = false;
    fms.steer_state.locked_course_err = 0;
}

// Helper to output to both k_rudder and k_steering servo functions
void mode_output_rudder_and_steering(float val)
{
    srv_channels_set_output_scaled(k_rudder, val);
    srv_channels_set_output_scaled(k_steering, val);
}

///
bool modeIsVtolManThrottle(mode_base_const_t mode)
{
#if 0 //HAL_QUADPLANE_ENABLED
    if (plane.quadplane.tailsitter.is_in_fw_flight() &&
        plane.quadplane.assisted_flight) {
        // We are a tailsitter that has fully transitioned to Q-assisted forward flight.
        // In this case the forward throttle directly drives the vertical throttle so
        // set vertical throttle state to match the forward throttle state. Confusingly the booleans are inverted,
        // forward throttle uses 'does_auto_throttle' whereas vertical uses 'is_vtol_man_throttle'.
        return !does_auto_throttle();
    }
#endif

    return false;
}

void modeUpdateTargetAltitude(mode_base_t mode)
{
    // TODO: ?
}

bool modePreArmChecks(mode_base_const_t mode, size_t buflen, char *buffer)
{
#if 0 //HAL_QUADPLANE_ENABLED
    if (plane.quadplane.enabled() && !is_vtol_mode() &&
            plane.quadplane.option_is_set(QuadPlane::OPTION::ONLY_ARM_IN_QMODE_OR_AUTO)) {
        hal.util->snprintf(buffer, buflen, "not Q mode");
        return false;
    }
#endif
    return true;
}

bool modeIsTakingOff(mode_base_const_t mode)
{
    (void)mode;
    return (fms.flight_stage == FWFS_TAKEOFF);
}

void modeRun(mode_base_t mode)
{
    // Direct stick mixing functionality has been removed, so as not to remove all stick mixing from the user completely
    // the old direct option is now used to enable fbw mixing, this is easier than doing a param conversion.
    if ((fms.g.stick_mixing == SM_FBW) || (fms.g.stick_mixing == SM_DIRECT_REMOVED)) {
        fms_stabilize_stick_mixing_fbw();
    }
    fms_stabilize_roll();
    fms_stabilize_pitch();
    fms_stabilize_yaw();
}
/*------------------------------------test------------------------------------*/


